#ifndef INS_MAP_H
#define INS_MAP_H

#endif //INS_MAP_H
#include <ros/ros.h>
#include <vector>
#include <math.h>
#include <queue>
#include <sstream>
#include <nav_msgs/OccupancyGrid.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
using namespace std;


class Map {
public:
    ros::Time map_load_time;            // 加载地图的时间戳
    double resolution;           //地图的分辨率，一个格子代表着多少米，[m/cell]
    unsigned int width;            // 地图的宽度，像素的个数, [cells]
    unsigned int height;                 // 地图的高度，像素的个数, [cells]
    int map_origin_x;     // 地图原点对应的物理世界的UTM x坐标
    int map_origin_y;    //地图原点对应的物理世界的UTM y坐标

    vector<vector<int8_t> > pixel; //概率栅格地图
    ros::Publisher map_pub;
    ros::Publisher path_pub;

    bool isInMap(int x,int y) const{
        return x>=0 && x<width && y>=0 && y < height;
    }
    Map(double resolution, unsigned int width, unsigned int height, int map_origin_x,int map_origin_y) : resolution(resolution),
                                                                                                         width(width),
                                                                                                         height(height),
                                                                                                         map_origin_x(map_origin_x),
                                                                                                         map_origin_y(map_origin_y){
        pixel.resize(height);
        for (auto &i: pixel) {
            i.resize(width,100);
        }
    }
    void setPublisher(ros::Publisher map_pub,ros::Publisher path_pub){
        this->map_pub = map_pub;
        this->path_pub = path_pub;
    }
    void update(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg){
        double x = (msg->pose.pose.position.x - map_origin_x) / resolution;
        double y = (msg->pose.pose.position.y - map_origin_y) / resolution;
        int x_floor_num = floor(x);//小
        int x_ceil_num = ceil(x);//大
        int y_floor_num = floor(y);//小
        int y_ceil_num = ceil(y);//大
        double x_prob_floor = abs(x - x_floor_num);
        double x_prob_ceil = abs(x - x_ceil_num);
        double y_prob_floor = abs(y - y_floor_num);
        double y_prob_ceil = abs(y - y_ceil_num);
        map_load_time = msg->header.stamp;
        //更新地图概率
        if(isInMap(x_floor_num,y_floor_num)) {
            if (pixel[x_floor_num][y_floor_num]>20) {
                pixel[x_floor_num][y_floor_num] *= x_prob_floor * y_prob_floor*0.9;
            }
            if(pixel[x_floor_num][y_floor_num]<5){
                pixel[x_floor_num][y_floor_num]=5;
            }
        }
        if(isInMap(x_floor_num,y_ceil_num)) {
            if (pixel[x_floor_num][y_ceil_num]>20) {
            pixel[x_floor_num][y_ceil_num] *= x_prob_floor * y_prob_ceil*0.9;
            }
            if(pixel[x_floor_num][y_ceil_num]<5){
                pixel[x_floor_num][y_ceil_num]=5;
            }
        }
        if(isInMap(x_ceil_num,y_ceil_num)) {
            if (pixel[x_ceil_num][y_ceil_num]>20) {
                pixel[x_ceil_num][y_ceil_num] *= x_prob_ceil * x_prob_ceil*0.9;
            }
            if (pixel[x_ceil_num][y_ceil_num]<5) {
                pixel[x_ceil_num][y_ceil_num]=5;
            }
        }
        if(isInMap(x_ceil_num,y_floor_num)) {
            if (pixel[x_ceil_num][y_floor_num]>20) {
                pixel[x_ceil_num][y_floor_num] *= x_prob_ceil * y_prob_floor*0.9;
            }
            if (pixel[x_ceil_num][y_floor_num]<5) {
                pixel[x_ceil_num][y_floor_num]=5;
            }
        }
        ROS_INFO("update x:%f y:%f",x,y);
        if (map_pub!= NULL){
            pushMapTopic();
        }
    }
    void pushMapTopic(){
        if (map_pub!= NULL) {
            nav_msgs::OccupancyGrid gridMap;

            // 定义地图消息参数
            gridMap.header.stamp = ros::Time::now();
            gridMap.info.resolution = resolution;
            gridMap.info.width = width;
            gridMap.info.height = height;
            gridMap.info.origin.position.x = map_origin_x;
            gridMap.info.origin.position.y = map_origin_y;
            gridMap.data.resize(gridMap.info.width * gridMap.info.height, 100);
            for (int i = 0; i < pixel.size(); i++) {
                for (int j = 0; j < pixel[0].size(); j++) {
                    gridMap.data[i + j * height] = pixel[i][j];
                }
            }
            map_pub.publish(gridMap);
            //ros::spinOnce();
        }
    }

    string pushPathTopic(std::deque<unsigned int> path){
        stringstream ss;
        // 创建一个nav_msgs::Path消息
        nav_msgs::Path path_msg;

        // 创建一个geometry_msgs::PoseStamped消息，用于添加到nav_msgs::Path消息中
        geometry_msgs::PoseStamped pose;

        // 设置路径的坐标系
        path_msg.header.frame_id = "base_footprint";

        // 设置路径上的点
        for(auto &data : path){
            int* position = new int[2];
            getPosition(data,position);
            ss<<"x:"<<position[0]<<'-'<<"y:"<<position[1]<<' ';
            pose.pose.position.x = position[0]*resolution;
            pose.pose.position.y = position[1]*resolution;
            pose.pose.orientation.w = 1.0;
            pose.header.stamp = ros::Time::now();
            path_msg.poses.push_back(pose);
        }
        if(path_pub != NULL) {
            path_pub.publish(path_msg);
        }
        return ss.str();
    }

    void getPosition(int order,int *res){
        res[0]= order/width;
        res[1]= order%width;
    }
};